/*
  FirmataParser.cpp
  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  See file LICENSE.txt for further informations on licensing terms.
*/

//******************************************************************************
//* Includes
//******************************************************************************

#include <plc_rte.h>

using namespace firmata;

//******************************************************************************
//* Constructors
//******************************************************************************

/**
 * The FirmataParser class.
 * @param dataBuffer A pointer to an external buffer used to store parsed data
 * @param dataBufferSize The size of the external buffer
 */
FirmataParser::FirmataParser(uint8_t *const dataBuffer, size_t dataBufferSize)
    : dataBuffer(dataBuffer),
      dataBufferSize(dataBufferSize),
      executeMultiByteCommand(0),
      multiByteChannel(0),
      waitForData(0),
      parsingSysex(false),
      sysexBytesRead(0),
      currentAnalogCallbackContext((void *)nullptr),
      currentDigitalCallbackContext((void *)nullptr),
      currentReportAnalogCallbackContext((void *)nullptr),
      currentReportDigitalCallbackContext((void *)nullptr),
      currentPinModeCallbackContext((void *)nullptr),
      currentPinValueCallbackContext((void *)nullptr),
      currentReportFirmwareCallbackContext((void *)nullptr),
      currentReportVersionCallbackContext((void *)nullptr),
      currentDataBufferOverflowCallbackContext((void *)nullptr),
      currentStringCallbackContext((void *)nullptr),
      currentSysexCallbackContext((void *)nullptr),
      currentSystemResetCallbackContext((void *)nullptr),
      currentAnalogCallback((callbackFunction)

                                nullptr),
      currentDigitalCallback((callbackFunction) nullptr),
      currentReportAnalogCallback((callbackFunction) nullptr),
      currentReportDigitalCallback((callbackFunction) nullptr),
      currentPinModeCallback((callbackFunction) nullptr),
      currentPinValueCallback((callbackFunction) nullptr),
      currentDataBufferOverflowCallback((dataBufferOverflowCallbackFunction)

                                            nullptr),
      currentStringCallback((stringCallbackFunction) nullptr),
      currentSysexCallback((sysexCallbackFunction) nullptr),
      currentReportFirmwareCallback((versionCallbackFunction) nullptr),
      currentReportVersionCallback((systemCallbackFunction) nullptr),
      currentSystemResetCallback((systemCallbackFunction) nullptr)
{
    allowBufferUpdate = ((uint8_t *)nullptr == dataBuffer);
}

//******************************************************************************
//* Public Methods
//******************************************************************************

//------------------------------------------------------------------------------
// Serial Receive Handling

/**
 * Parse data from the input stream.
 * @param inputData A single byte to be added to the parser.
 */
extern "C" u16 crc_16(u8 *buf, int cnt, u16 init);

void FirmataParser::parse(nStream *stream, uint8_t inputData)
{
    uint8_t command;
#ifdef FORWARD_NRS
    if (stream->flag & FLAG_FORWARD)
    {
        stream->forward(inputData);
        if (inputData == END_SYSEX || rtos::ticks() > stream->tick_forward)
        {
            stream->closeForward();
        }
        return;
    }
    if (forward > 0)
    {
        board.forwardMessage(forward, inputData);
        if (inputData == END_SYSEX + forward)
        {
            forward = 0;
        }
        return;
    }
#endif
    if (parsingSysex)
    {
        if (inputData == END_SYSEX)
        {
            // stop sysex byte
            parsingSysex = false;
            u16 crc1 = 0, crc = 0;
#ifdef FIRMATA_USE_CRC
            if (sysexBytesRead > 3)
            {
                crc1 = (dataBuffer[sysexBytesRead - 1] << 14) + (dataBuffer[sysexBytesRead - 2] << 7) + dataBuffer[sysexBytesRead - 3];
                crc = crc_16(dataBuffer+1, sysexBytesRead - 4, 0);
                sysexBytesRead -= 3;
            }
#endif
            if (crc == crc1)
            {
                // fire off handler function
                processSysexMessage(stream);
            }
            stream->flag &= ~FLAG_SYSEX;
        }
        else
        {
            // normal data byte - add to buffer
            bufferDataAtPosition(stream, inputData, sysexBytesRead);
            ++sysexBytesRead;
        }
    }
    else if ((waitForData > 0) && (inputData < 128))
    {
        --waitForData;
        bufferDataAtPosition(stream, inputData, waitForData);
        if ((waitForData == 0) && executeMultiByteCommand)
        { // got the whole message
            switch (executeMultiByteCommand)
            {
#ifdef FIRMATA_FAST
            case ANALOG_MESSAGE:
                if (currentAnalogCallback)
                {
                    (*currentAnalogCallback)(currentAnalogCallbackContext, stream,
                                             multiByteChannel,
                                             (uint16_t)(dataBuffer[0] << 7) + dataBuffer[1]);
                }
                break;
            case DIGITAL_MESSAGE:
                if (currentDigitalCallback)
                {
                    (*currentDigitalCallback)(currentDigitalCallbackContext, stream,
                                              multiByteChannel,
                                              (uint16_t)(dataBuffer[0] << 7) + dataBuffer[1]);
                }
                break;
            case SET_PIN_MODE:
                if (currentPinModeCallback)
                    (*currentPinModeCallback)(currentPinModeCallbackContext, stream, dataBuffer[1], dataBuffer[0]);
                break;
            case SET_DIGITAL_PIN_VALUE:
                if (currentPinValueCallback)
                    (*currentPinValueCallback)(currentPinValueCallbackContext, stream, dataBuffer[1],
                                               dataBuffer[0]);
                break;
            case REPORT_ANALOG:
                if (currentReportAnalogCallback)
                    (*currentReportAnalogCallback)(currentReportAnalogCallbackContext, stream, multiByteChannel,
                                                   dataBuffer[0]);
                break;
            case REPORT_DIGITAL:
                if (currentReportDigitalCallback)
                    (*currentReportDigitalCallback)(currentReportDigitalCallbackContext, stream, multiByteChannel,
                                                    dataBuffer[0]);
                break;
#endif
            default:
                break;
            }
            executeMultiByteCommand = 0;
        }
    }
    else
    {
        // remove channel info from command byte if less than 0xF0
        if (inputData < 0xF0)
        {
            command = inputData & 0xF0;
            multiByteChannel = inputData & 0x0F;
        }
        else
        {
            command = inputData;
            // commands in the 0xF* range don't use channel data
        }
        switch (command)
        {
#ifdef FIRMATA_FAST
        case ANALOG_MESSAGE:
        case DIGITAL_MESSAGE:
        case SET_PIN_MODE:
        case SET_DIGITAL_PIN_VALUE:
            waitForData = 2; // two data bytes needed
            executeMultiByteCommand = command;
            break;
        case REPORT_ANALOG:
        case REPORT_DIGITAL:
            waitForData = 1; // one data byte needed
            executeMultiByteCommand = command;
            break;
        case SYSTEM_RESET:
            systemReset(stream);
            break;
        case REPORT_VERSION:
            if (currentReportVersionCallback)
                (*currentReportVersionCallback)(currentReportVersionCallbackContext, stream);
            break;
#endif
        case START_SYSEX:
            parsingSysex = true;
            sysexBytesRead = 0;
            break;
        default:
#ifdef FORWARD_NRS
            if (command > START_SYSEX && command < START_SYSEX + FORWARD_NRS)
            {
                forward = command - START_SYSEX;
                forwardMessage(forward, 0xf0);
            }
#endif
            break;
        }
    }
}

/**
 * @return Returns true if the parser is actively parsing data.
 */
bool FirmataParser::isParsingMessage(void)
    const
{
    return (waitForData > 0 || parsingSysex);
}

/**
 * Provides a mechanism to either set or update the working buffer of the parser.
 * The method will be enabled when no buffer has been provided, or an overflow
 * condition exists.
 * @param Buffer A pointer to an external buffer used to store parsed data
 * @param BufferSize The size of the external buffer
 */
int FirmataParser::setDataBufferOfSize(uint8_t *Buffer, size_t BufferSize)
{
    int result;

    if (!allowBufferUpdate)
    {
        result = __LINE__;
    }
    else if ((uint8_t *)nullptr == Buffer)
    {
        result = __LINE__;
    }
    else
    {
        this->dataBuffer = Buffer;
        this->dataBufferSize = BufferSize;
        allowBufferUpdate = false;
        result = 0;
    }

    return result;
}

/**
 * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
 * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
 * @param command The ID of the command to attach a callback function to.
 * @param newFunction A reference to the callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(uint8_t command, callbackFunction newFunction, void *context)
{
    switch (command)
    {
    case ANALOG_MESSAGE:
        currentAnalogCallback = newFunction;
        currentAnalogCallbackContext = context;
        break;
    case DIGITAL_MESSAGE:
        currentDigitalCallback = newFunction;
        currentDigitalCallbackContext = context;
        break;
    case REPORT_ANALOG:
        currentReportAnalogCallback = newFunction;
        currentReportAnalogCallbackContext = context;
        break;
    case REPORT_DIGITAL:
        currentReportDigitalCallback = newFunction;
        currentReportDigitalCallbackContext = context;
        break;
    case SET_PIN_MODE:
        currentPinModeCallback = newFunction;
        currentPinModeCallbackContext = context;
        break;
    case SET_DIGITAL_PIN_VALUE:
        currentPinValueCallback = newFunction;
        currentPinValueCallbackContext = context;
        break;
    default:
        break;
    }
}

/**
 * Attach a version callback function (supported option: REPORT_FIRMWARE).
 * @param command The ID of the command to attach a callback function to.
 * @param newFunction A reference to the callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(uint8_t command, versionCallbackFunction newFunction, void *context)
{
    switch (command)
    {
    case REPORT_FIRMWARE:
        currentReportFirmwareCallback = newFunction;
        currentReportFirmwareCallbackContext = context;
        break;
    default:
        break;
    }
}

/**
 * Attach a system callback function (supported options are: SYSTEM_RESET, REPORT_VERSION).
 * @param command The ID of the command to attach a callback function to.
 * @param newFunction A reference to the callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(uint8_t command, systemCallbackFunction newFunction, void *context)
{
    switch (command)
    {
    case REPORT_VERSION:
        currentReportVersionCallback = newFunction;
        currentReportVersionCallbackContext = context;
        break;
    case SYSTEM_RESET:
        currentSystemResetCallback = newFunction;
        currentSystemResetCallbackContext = context;
        break;
    default:
        break;
    }
}

/**
 * Attach a callback function for the STRING_DATA command.
 * @param command Must be set to STRING_DATA or it will be ignored.
 * @param newFunction A reference to the string callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(uint8_t command, stringCallbackFunction newFunction, void *context)
{
    switch (command)
    {
    case STRING_DATA:
        currentStringCallback = newFunction;
        currentStringCallbackContext = context;
        break;
    default:
        break;
    }
}

/**
 * Attach a generic sysex callback function to sysex command.
 * @param command The ID of the command to attach a callback function to.
 * @param newFunction A reference to the sysex callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(uint8_t command, sysexCallbackFunction newFunction, void *context)
{
    (void)command;
    currentSysexCallback = newFunction;
    currentSysexCallbackContext = context;
}

/**
 * Attach a buffer overflow callback
 * @param newFunction A reference to the buffer overflow callback function to attach.
 * @param context An optional context to be provided to the callback function (nullptr by default).
 * @note The context parameter is provided so you can pass a parameter, by reference, to
 *       your callback function.
 */
void FirmataParser::attach(dataBufferOverflowCallbackFunction
                               newFunction,
                           void *context)
{
    currentDataBufferOverflowCallback = newFunction;
    currentDataBufferOverflowCallbackContext = context;
}

/**
 * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
 * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
 * @param command The ID of the command to detatch the callback function from.
 */
void FirmataParser::detach(uint8_t command)
{
    switch (command)
    {
    case REPORT_FIRMWARE:
        attach(command, (versionCallbackFunction) nullptr, nullptr);
        break;
    case REPORT_VERSION:
    case SYSTEM_RESET:
        attach(command, (systemCallbackFunction) nullptr, nullptr);
        break;
    case STRING_DATA:
        attach(command, (stringCallbackFunction) nullptr, nullptr);
        break;
    case START_SYSEX:
        attach(command, (sysexCallbackFunction) nullptr, nullptr);
        break;
    default:
        attach(command, (callbackFunction) nullptr, nullptr);
        break;
    }
}

/**
 * Detach the buffer overflow callback
 * @param <unused> Any pointer of type dataBufferOverflowCallbackFunction.
 */
void FirmataParser::detach(dataBufferOverflowCallbackFunction)
{
    currentDataBufferOverflowCallback = (dataBufferOverflowCallbackFunction) nullptr;
    currentDataBufferOverflowCallbackContext = (void *)nullptr;
}

//******************************************************************************
//* Private Methods
//******************************************************************************

/**
 * Buffer abstraction to prevent memory corruption
 * @param data The byte to put into the buffer
 * @param pos The position to insert the byte into the buffer
 * @return writeError A boolean to indicate if an error occured
 * @private
 */
bool FirmataParser::bufferDataAtPosition(nStream *stream, const uint8_t data, const size_t pos)
{
    bool bufferOverflow = (pos >= dataBufferSize);

    // Notify of overflow condition
    if (bufferOverflow && ((dataBufferOverflowCallbackFunction) nullptr != currentDataBufferOverflowCallback))
    {
        allowBufferUpdate = true;
        currentDataBufferOverflowCallback(currentDataBufferOverflowCallbackContext, stream);
        // Check if overflow was resolved during callback
        bufferOverflow = (pos >= dataBufferSize);
    }

    // Write data to buffer if no overflow condition persist
    if (!bufferOverflow)
    {
        dataBuffer[pos] = data;
    }

    return bufferOverflow;
}

/**
 * Transform 7-bit firmata message into 8-bit stream
 * @param bytec The encoded data byte length of the message (max: 16383).
 * @param bytev A pointer to the encoded array of data bytes.
 * @return The length of the decoded data.
 * @note The conversion will be done in place on the provided buffer.
 * @private
 */
size_t FirmataParser::decodeByteStream(size_t bytec, uint8_t *bytev)
{
    size_t decoded_bytes, i;

    for (i = 0, decoded_bytes = 0; i < bytec; ++decoded_bytes, ++i)
    {
        bytev[decoded_bytes] = bytev[i];
        bytev[decoded_bytes] |= (uint8_t)(bytev[++i] << 7);
    }

    return decoded_bytes;
}

/**
 * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
 * Calls callback function for STRING_DATA and all other sysex messages.
 * @private
 */
void FirmataParser::processSysexMessage(nStream *stream)
{
    switch (dataBuffer[0])
    { // first byte in buffer is command
    case REPORT_FIRMWARE:
        if (currentReportFirmwareCallback)
        {
            const size_t major_version_offset = 1;
            const size_t minor_version_offset = 2;
            const size_t string_offset = 3;
            // Test for malformed REPORT_FIRMWARE message (used to query firmware prior to Firmata v3.0.0)
            if (3 > sysexBytesRead)
            {
                (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, stream, 0, 0,
                                                 (const char *)nullptr);
            }
            else
            {
                const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset),
                                                                               &dataBuffer[string_offset]));
                bufferDataAtPosition(stream, '\0', end_of_string); // nullptr terminate the string
                (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, stream,
                                                 (size_t)dataBuffer[major_version_offset],
                                                 (size_t)dataBuffer[minor_version_offset],
                                                 (const char *)&dataBuffer[string_offset]);
            }
        }
        break;

    case STRING_DATA:
        if (currentStringCallback)
        {
            const size_t string_offset = 1;
            // const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset),
            //                                                                &dataBuffer[string_offset]));
            // bufferDataAtPosition('\0', end_of_string); // nullptr terminate the string
            (*currentStringCallback)(currentStringCallbackContext, stream,
                                     (const char *)&dataBuffer[string_offset]);
        }
        break;
    default:
        if (currentSysexCallback)
            (*currentSysexCallback)(currentSysexCallbackContext, stream, dataBuffer[0], sysexBytesRead - 1,
                                    dataBuffer + 1);
    }
}

/**
 * Resets the system state upon a SYSTEM_RESET message from the host software.
 * @private
 */
void FirmataParser::systemReset(nStream *stream)
{

    waitForData = 0;             // this flag says the next serial input will be data
    executeMultiByteCommand = 0; // execute this after getting multi-byte data
    multiByteChannel = 0;        // channel data for multiByteCommands

    for (int i = 0; i < dataBufferSize; ++i)
    {
        dataBuffer[i] = 0;
    }

    parsingSysex = false;
    sysexBytesRead = 0;

    if (currentSystemResetCallback)
        (*currentSystemResetCallback)(currentSystemResetCallbackContext, stream);
}
